/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "vrepcontrol.h"

#include <stdio.h>
#include <stdlib.h>
#include "extIk.h"
extern "C" {
#include "extApi.h"
}


VrepControl::VrepControl(E_ECAT_REAL_VREP_MODE ioSmiulationModeIn,
                         E_ECAT_REAL_VREP_MODE motorSmiulationModeIn,QObject *parent)
{

    versionInfo.componentName="VrepControl";
    versionInfo.componentVersion="V1.0";
    versionInfo.publishDate="2018.10.27";

    isRun=false;
    interval=30;//
    clientID = -1;
    ioSmiulationMode=ioSmiulationModeIn;
    motorSmiulationMode=motorSmiulationModeIn;

    for(int i=0;i<D_SUPPORT_SIMULATION_ROBOT_MAX;i++)
    {
       vob[i]=new VrepObject();
    }
    read();
    isRun=true;

    timer=new QTimer();
//    connect( timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
    createThread();

}
VrepControl::~VrepControl()
{
    quit();
}

int VrepControl::readVersion(std::vector <ComponentVersion> &componentVersionList)
{

    componentVersionList.push_back(versionInfo);
    return 1;
}

void VrepControl::timeOut_slot()
{
     demo();
}

//端口有点奇怪，对不上也能控制
int VrepControl::connectToVrep( QString _ip, int _port )
{
    QByteArray ba;
    ba.append( _ip);
    char* vrepip= ba.data();
    clientID=simxStart(vrepip, _port, true, true, 1000, 2);
    if (clientID == -1)
    {
        qDebug()<< vrepip<<_port;
        displayMsg("connect to vrep err !",-1);
        return 0;
    }
    vrepIp=_ip;
    vrepPort=_port;
#if D_USE_SYNCHRONIZED_MODE
    simxSynchronous(clientID,true); // Enable the synchronous mode (Blocking function call)
#endif
    simxStartSimulation(clientID,simx_opmode_oneshot); //启动模拟
    displayMsg("connect to vrep success !", clientID);
    return 1;
}

void VrepControl::disconnectFromVrep( )
{

#if D_USE_SYNCHRONIZED_MODE
    simxSynchronous(clientID,false);//关闭同步
#endif
#if D_HAS_IK_MODE
    simEmbShutDown(); // End the external IK
#endif
    simxFinish(clientID); // End the remote API
    clientID=-1;
}

void VrepControl::displayMsg( QString msg ,int code )
{
#if 0
    qDebug()<< code << msg;
#else
    emit signal_msg( msg, code );
#endif
}

void VrepControl::read()
{
    //ＴＯＤＯ：　vob[i]怎么和 vrep scene中的多个机器人对应
    for(int i=0;i<D_SUPPORT_SIMULATION_ROBOT_MAX;i++)
    {
        if( !vob[i] || vob[i]->robotChoose==-1 )
            continue;
        // if(!getChanged){
        int ret=0;
        bool getok=true;
        for(int j=0; j<vob[i]->axisNum; j++)
        {
            if( vob[i]->obHandles[j]==0 ){
                getok=false;
                continue;
            }
            ret = simxGetJointPosition( clientID, vob[i]->obHandles[j], (simxFloat*)(&vob[i]->getPos[j]), simx_opmode_streaming );
            if(ret<0){
                displayMsg(  "simxGetJointPosition  error!", ret );
                continue;
            }
        }
        if( getok )
            vob[i]->getChanged=true;
        //  emit vob[i]->signal_dataChanged();
    }
}

void VrepControl::start(int ms)
{

    timer->start( ms );
}


void VrepControl::quit()
{
    isRun=false;
    disconnectFromVrep();
    for(int i=0;i<D_SUPPORT_SIMULATION_ROBOT_MAX;i++){
        if(vob[i] )
            vob[i]->deleteLater();
    }
//        QThread::quit();
}

void VrepControl::write()
{
    //ＴＯＤＯ：　vob[i]怎么和 vrep scene中的多个机器人对应
    if(simxGetConnectionId(clientID)!=-1)
    {
        simxPauseCommunication( clientID,1 );
        for(int i=0;i<D_SUPPORT_SIMULATION_ROBOT_MAX;i++)
        {
            if( !vob[i] || vob[i]->robotChoose==-1 )
                continue;
            vob[i]->test_demo();

            if( vob[i]->setChanged )
            {
                int ret=0;
                if(vob[i]->robotChoose == LBR_iiwa_7_R800|| vob[i]->robotChoose==UR5|| vob[i]->robotChoose==UARM)
                {

                    for(int j=0; j<vob[i]->axisNum; j++)
                    {
                        ret  =  simxSetJointTargetPosition(clientID, vob[i]->obHandles[j], vob[i]->setPos[j], simx_opmode_oneshot);
                        if( ret<0 ){
                            displayMsg( "simxSetJointPosition  error!", ret);
                        }
                    }
                }
                else
                {
                    for(int j=0; j<vob[i]->axisNum; j++)
                    {
                        ret =  simxSetJointPosition(clientID, vob[i]->obHandles[j], vob[i]->setPos[j], simx_opmode_oneshot);
                        if( ret<0 ){
                            displayMsg( "simxSetJointPosition  error!", ret);
                        }
                    }
                }
                vob[i]->setChanged=false;
            }
        }
        simxPauseCommunication(clientID,0);
#if D_USE_SYNCHRONIZED_MODE
        simxSynchronousTrigger(clientID);//调用延时很大
#endif
    }
}


void VrepControl::demo()
{
    if( clientID==-1 )
        return;

#if D_CHECK_SYNC_TIME
    //时间测量
    struct timespec time_start={0, 0},time_end={0, 0};
    clock_gettime(CLOCK_REALTIME, &time_start);
    struct timeval start, end;
    gettimeofday( &start, NULL );
#endif

    read();
    write();

#if 0
    if (simxGetConnectionId(clientID)!=-1 )
    {
        simxInt pingTime;
        simxGetPingTime(clientID, &pingTime); // After this call, the first simulation step is finished (Blocking function call)
        qDebug()<<"pingTime: "<<pingTime;
    }
#endif

#if D_CHECK_SYNC_TIME
    //时间测量
    unsigned int interval=0;
    clock_gettime(CLOCK_REALTIME, &time_end);
    interval = time_end.tv_nsec-time_start.tv_nsec;
    gettimeofday( &end, NULL );

    interval = end.tv_usec- start.tv_usec;
    qDebug()<< "xxxxxxxxxx=interval: "<<interval;//测试i3用时约10～20ms
#endif

}


#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* VrepControl::threadRun(void *)
{
    printf("VrepControl theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 50;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************VrepControl pid="<<gettid();
    usleep(659000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);

    QThread::msleep(50);
    while(1)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
         QThread::msleep(50);
         demo();


    }
    printf("VrepControl thread quit!!!");
}

void* VrepControl::threadTask( void* classPtr )
{
    qDebug()<<"VrepControl thread run-------------------------";
    return ((VrepControl*)classPtr)->threadRun(NULL);
}

void VrepControl::createThread()
{
    pthread_t threadId;
    int ret=pthread_create( &threadId, NULL, &VrepControl::threadTask,this);
    qDebug()<<"VrepControl createThread-------------------------";
}
